package com.tmrnavi.myapplication;

import androidx.annotation.NonNull;
import androidx.appcompat.app.AppCompatActivity;
import androidx.core.app.ActivityCompat;
import androidx.core.content.ContextCompat;
import android.Manifest;
import android.annotation.SuppressLint;
import android.content.Context;
import android.content.Intent;
import android.content.SharedPreferences;
import android.content.pm.PackageManager;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.location.Location;
import android.location.LocationListener;
import android.location.LocationManager;
import android.location.OnNmeaMessageListener;
import android.net.Uri;
import android.os.Bundle;
import android.os.Handler;
import android.os.Message;
import android.os.PowerManager;
import android.view.View;
import android.widget.Button;
import android.widget.TextView;
import android.widget.Toast;

import java.util.Timer;
import java.util.TimerTask;

public class MainActivity extends AppCompatActivity {
    // Used to load the 'myapplication' library on application startup.
    static {
        System.loadLibrary("myapplication");
    }

    public TextView tv_gyro_x, tv_gyro_y, tv_gyro_z;        // 一大堆显示控件
    public TextView tv_acce_x, tv_acce_y, tv_acce_z;
    public TextView tv_magn_x, tv_magn_y, tv_magn_z;
    public TextView tv_atti_x, tv_atti_y, tv_atti_z;
    public TextView tv_veln_x, tv_veln_y, tv_veln_z;
    public TextView tv_posi_x, tv_posi_y, tv_posi_z;
    public TextView tv_time, tv_baro, tv_presAlt;
    public TextView tv_gnss_time, tv_gnss_lat, tv_gnss_lng, tv_gnss_alt, tv_gnss_vhor, tv_gnss_head;    // GNSS数据控件
    public Button btn_nav, btn_save, btn_cop, btn_about, btn_data, btn_share, btn_kfPara;               // 三个按钮
    public static SensorManager sensorManager_IMU;          // Sensor Manager对象
    public static Sensor sensor_gyro, sensor_acce, sensor_magn, sensor_att, sensor_pre;     // 四个Sensor
    public static SensorEventListener sensorEventListener;      // 手机传感器监听器
    public View.OnClickListener buttonListener;                 // 按键监听器
    public static long t_stp, t0_stp;                           // 导航更新时间戳
    public static float t = 0.0f;                               // 导航时长
    public static LocationManager locationManager;              // LocationManager GNSS定位用
    public static LocationListener locationListener;            // GNSS定位监听器
    public static OnNmeaMessageListener nmeaMessageListener;    // 获取速度信息用

    // 消息类型
    public static final int DISPMSG_SENSOR = 101;               // 显示传感器数据
    public static final int DISPMSG_AHRS = 102;                 // 显示航姿数据
    public static final int DISPMSG_SINS = 103;                 // 显示导航数据
    public static final int DISPMSG_XKPK = 104;                 // 2.0.1后不再显示KF数据
    private static boolean saveOn = false;                      // 数据保存标志

    // 传感器数据初始化
    private float[] wib = new float[]{0.0f, 0.0f, 0.0f}, avg_wib = new float[3];    // 角速度与平滑后角速度
    private float[] fsf = new float[]{0.0f, 0.0f, 0.0f}, avg_fsf = new float[3];    // 加速度与平滑后加速度
    private float[] mag = new float[]{0.0f, 0.0f, 0.0f}, avg_mag = new float[3];    // 磁场与平滑后磁场
    private float pre = 0, avg_pre = 0, presAlt = 0.0f;                             // 气压、平滑后气压与气压高度
    private float[] qnb = new float[]{0.0f, 0.0f, 0.0f, 0.0f}, att = new float[3];     // quaternion
    private float[] fsf0 = new float[3];                // 用于降噪
    private double[] avppx = new double[10+16+16+1];
    private int[] CNT = new int[]{0, 0, 0, 0};          // 计数器
    private boolean IMUUPDATE = false;                  // IMU 数据更新标志
    private int   INSCNT = 0;                           // INS计数
    private final float PI = 3.141592653f;
    private final float DEG = PI / 180;
    private final float SEC = DEG/3600;
    private final float MG = (float) (9.8/1000.0);
    private static double[] pos0 = new double[]{0.597706293396019, 1.90083222404074, 400};
    private static double[] vn0 = new double[]{0.0,0.0,0.0};
    private final double[] gvp0 = new double[]{0,0, 0.597706293396019, 1.90083222404074, 2, 0.0,0.0,0.0, 0};
    private final double[] R_eth = new double[]{6300000, 6300000*Math.cos(0.597706293396019)};
    private double[] gnssData = new double[]{2.0,0,0.597706293396019, 1.90083222404074, 400, 0,0,0, 0.1};
    private float[] kfParas = new float[27];
    private boolean velocityUpdate = false;
    private double[] xkpkk = new double[33];
    private LPFilter lpFilterW, lpFilterF, lpFilterM;                                       // 低通滤波器
    private final PSINSDataStore psinsDataStore = new PSINSDataStore(this);     // 惯导数据存储对象

    private static Timer naviTimer;             // 定时器
    private static MsgHandler msgHandler;       // 消息处理Handler
    private static int k = 0;                   // 采样计数
    private final int IDLESTATE = 400;          // 空闲状态标志
    private final int ALIGNSTATE = 100;         // 对准状态标志
    private final int CPNAVRDY   = 200;         // 组合导航准备标志
    private final int CPNAVSTATE = 201;         // 组合导航状态标志
    private static int navState;                // 导航状态变量
    //private static boolean SimulationON = true; // 模拟静基座

    private static final int OPEN_SET_REQUEST_CODE = 100;
    PowerManager.WakeLock m_wklk;

    @SuppressLint({"MissingPermission", "InvalidWakeLockTag"})
    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);

        /*             保持屏幕唤醒               */
        PowerManager pm = (PowerManager)getSystemService(POWER_SERVICE);
        m_wklk = pm.newWakeLock(PowerManager.SCREEN_DIM_WAKE_LOCK|PowerManager.ACQUIRE_CAUSES_WAKEUP, "SensorWorking");
        m_wklk.acquire(); //设置保持唤醒

        /*              控件初始化               */
        tv_gyro_x = findViewById(R.id.tv_gyro_x);
        tv_gyro_y = findViewById(R.id.tv_gyro_y);
        tv_gyro_z = findViewById(R.id.tv_gyro_z);
        tv_acce_x = findViewById(R.id.tv_acce_x);
        tv_acce_y = findViewById(R.id.tv_acce_y);
        tv_acce_z = findViewById(R.id.tv_acce_z);
        tv_magn_x = findViewById(R.id.tv_magn_x);
        tv_magn_y = findViewById(R.id.tv_magn_y);
        tv_magn_z = findViewById(R.id.tv_magn_z);
        tv_atti_x = findViewById(R.id.tv_atti_x);
        tv_atti_y = findViewById(R.id.tv_atti_y);
        tv_atti_z = findViewById(R.id.tv_atti_z);
        tv_veln_x = findViewById(R.id.tv_veln_x);
        tv_veln_y = findViewById(R.id.tv_veln_y);
        tv_veln_z = findViewById(R.id.tv_veln_z);
        tv_posi_x = findViewById(R.id.tv_posi_x);
        tv_posi_y = findViewById(R.id.tv_posi_y);
        tv_posi_z = findViewById(R.id.tv_posi_z);
        tv_time = findViewById(R.id.tv_time);
        tv_baro = findViewById(R.id.tv_baro);
        tv_presAlt = findViewById(R.id.tv_presAlt);
        btn_nav = findViewById(R.id.btn_nav);
        btn_save = findViewById(R.id.btn_save);
        btn_cop = findViewById(R.id.btn_cop);
        btn_about = findViewById(R.id.btn_about);
        btn_data = findViewById(R.id.btn_dat);
        btn_share = findViewById(R.id.btn_share);
        btn_kfPara = findViewById(R.id.btn_kfPara);

        tv_gnss_time = findViewById(R.id.tv_gnss_time);
        tv_gnss_lat = findViewById(R.id.tv_gnss_lat);
        tv_gnss_lng = findViewById(R.id.tv_gnss_lng);
        tv_gnss_alt = findViewById(R.id.tv_gnss_alt);
        tv_gnss_vhor = findViewById(R.id.tv_gnss_vhor);
        tv_gnss_head = findViewById(R.id.tv_gnss_head);
        /* ******************************************************************* */

        // 请求权限
        int hasPermission = ContextCompat.checkSelfPermission(getApplication(),
                Manifest.permission.ACCESS_FINE_LOCATION);
        if (hasPermission != PackageManager.PERMISSION_GRANTED) {
            ActivityCompat.requestPermissions(this, new String[]{Manifest.permission.ACCESS_FINE_LOCATION},
                    OPEN_SET_REQUEST_CODE);
        }

        // 初始化Sensor对象
        sensorManager_IMU = (SensorManager) getSystemService(Context.SENSOR_SERVICE);
        sensor_gyro = sensorManager_IMU.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
        sensor_acce = sensorManager_IMU.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
        sensor_magn = sensorManager_IMU.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
        sensor_att  = sensorManager_IMU.getDefaultSensor(Sensor.TYPE_ROTATION_VECTOR);
        sensor_att  = sensorManager_IMU.getDefaultSensor(Sensor.TYPE_PRESSURE);

        naviTimer = new Timer();            // Navigation Timer Initialization
        msgHandler = new MsgHandler();      // 消息处理Handler初始化
        lpFilterW = new LPFilter();         // 三个低通滤波器
        lpFilterF = new LPFilter();
        lpFilterM = new LPFilter();
        navState = IDLESTATE;               // 设置为空闲状态

        // 接收传感器数据
        sensorEventListener = new SensorEventListener() {
            @Override
            public void onSensorChanged(SensorEvent event) {
                switch (event.sensor.getType()) {
                    case Sensor.TYPE_GYROSCOPE:         // 陀螺输出角速度
                        wib[0] += event.values[0];
                        wib[1] += event.values[1];
                        wib[2] += event.values[2];
                        CNT[0]++;
                        break;
                    case Sensor.TYPE_ACCELEROMETER:     // 加计输出比力
                        fsf[0] += event.values[0];
                        fsf[1] += event.values[1];
                        fsf[2] += event.values[2];
                        CNT[1]++;
                        break;
                    case Sensor.TYPE_MAGNETIC_FIELD:    // 磁场强度向量
                        mag[0] += event.values[0];
                        mag[1] += event.values[1];
                        mag[2] += event.values[2];
                        CNT[2]++;
                        break;
                    case Sensor.TYPE_ROTATION_VECTOR:   // 姿态四元数（Android计算姿态结果）
                        qnb[0] = event.values[3];
                        qnb[1] = event.values[0];
                        qnb[2] = event.values[1];
                        qnb[3] = event.values[2];
                        break;
                    case Sensor.TYPE_PRESSURE:          // 气压
                        pre += event.values[0];
                        CNT[3]++;
                        break;
                }
            }

            @Override
            public void onAccuracyChanged(Sensor sensor, int accuracy) {
            }
        };

        // 注册传感器
        sensorManager_IMU.registerListener(sensorEventListener, sensor_gyro, SensorManager.SENSOR_DELAY_FASTEST);
        sensorManager_IMU.registerListener(sensorEventListener, sensor_acce, SensorManager.SENSOR_DELAY_FASTEST);
        sensorManager_IMU.registerListener(sensorEventListener, sensor_magn, SensorManager.SENSOR_DELAY_FASTEST);
        sensorManager_IMU.registerListener(sensorEventListener, sensor_att, SensorManager.SENSOR_DELAY_FASTEST);
        sensorManager_IMU.registerListener(sensorEventListener, sensor_pre, SensorManager.SENSOR_DELAY_FASTEST);

        // NMEA message Update——读取速度信息
        nmeaMessageListener = new OnNmeaMessageListener() {
            @Override
            public void onNmeaMessage(String message, long timestamp) {
                if (message.substring(0,6).equals("$GPVTG") || message.substring(0,6).equals("$GNVTG")){
                    String[] contents = message.split(",");
                    if (contents[7].length()>0){
                        double yaw = 0.0;
                        if (contents[1].length()>0) {
                            yaw = Double.parseDouble(contents[1]);              // 航向是北偏东
                        }
                        double vh = Double.parseDouble(contents[7])/3.6;        // ！！注意！！ 速度单位km/s
                        gnssData[5] = vh*Math.sin(yaw*DEG);     // 东向速度
                        gnssData[6] = vh*Math.cos(yaw*DEG);     // 北向速度
                        gnssData[7] = 0;                        // 没有天向速度，所以全部置零
                        //Log.v("NMEA","Yaw=" + yaw + ",  vE=" + gnssData[5] + ",  vN=" + gnssData[6]);
                        velocityUpdate = true;
                        GNSSVelDisp(new double[]{vh, yaw});     // 更新GNSS速度显示
                    } else {
                        velocityUpdate = false;
                    }
                }
            }
        };

        // GNSS数据更新——读取位置信息
        locationListener = new LocationListener() {
            @Override
            public void onLocationChanged(@NonNull Location location) {
                gnssData[1] = (double)(location.getTime()/1000)%10000;      // UTC时间
                gnssData[2] = location.getLatitude()*DEG;                   // 纬度
                gnssData[3] = location.getLongitude()*DEG;                  // 经度
                gnssData[4] = location.getAltitude();                       // 高度（椭球高）
                double vsum = location.getSpeed();                          // 水平速度

                if (velocityUpdate) {                   // 速度更新
                    velocityUpdate = false;
                    gnssData[0] = 2.0;                  // 2 表示有速度量测
                } else {
                    gnssData[0] = 1.0;                  // 1 表示只有位置量测
                }

                gnssData[8] = location.getAccuracy();   // 定位经度
                GNSSDataDisp(gnssData);
                if (navState == CPNAVRDY) {             // 组合导航准备状态：
                    pos0[0] = gnssData[2];              // 修改pos0的值
                    pos0[1] = gnssData[3];
                    pos0[2] = gnssData[4];
                    KFAPP_Init(vn0, pos0);              // 当GNSS有效时，完成组合导航系统初始化
                    readKFParas();                      // 读取设置的KF参数
                    btn_save.setText("停止保存");
                    psinsDataStore.open();              // 打开数据保存功能
                    saveOn = true;
                    navState = CPNAVSTATE;              // 初始化后切换到组合导航状态
                } else if (navState == CPNAVSTATE) {    // 如果时组合导航状态则进行量测更新，接收返回的滤波参数
                    xkpkk = KFAPP_MUpdate(gnssData);    // 量测更新
//                    Message msg1 = msgHandler.obtainMessage();  // 发送消息更新XkPk
//                    msg1.arg1 = DISPMSG_XKPK;
//                    msgHandler.sendMessage(msg1);
                }
            }
        };

        // 设置NMEA和GNSS数据读取
        locationManager = (LocationManager) getSystemService(LOCATION_SERVICE);
        locationManager.addNmeaListener(nmeaMessageListener);
        locationManager.requestLocationUpdates(LocationManager.GPS_PROVIDER,
                500, 0, locationListener);      // 加入监听

        // 定时器任务用于定时输出
        TimerTask IMUdataTask = new TimerTask() {
            @Override
            public void run() {
                t_stp = System.currentTimeMillis();
                t_stp = t_stp - t0_stp;
                t = t_stp / 1000.0f;
                if (CNT[0] != 0 && CNT[1] != 0 && CNT[2] != 0) {    // 采样周期内的传感器数据取平均
                    for (int i = 0; i < 3; i++) {                   // 气压计的频率太低，所以不放在一起
                        avg_wib[i] = wib[i] / CNT[0] / DEG;
                        wib[i] = 0;
                        avg_fsf[i] = fsf[i] / CNT[1];
                        fsf[i] = 0;
                        avg_mag[i] = mag[i] / CNT[2];
                        mag[i] = 0;
                    }
//                    Log.v("snesor_cnt", CNT[0]+"\t"+CNT[1]+"\t"+CNT[2]+"\t"+CNT[3]);
                    if (CNT[3]!=0) {                        // 目前仅读取数据
                        avg_pre = pre / CNT[3];
                        pre = 0;
                        /* ****** 计算气压高度 ******* */
                        presAlt = (float) (44300*(1- Math.pow((avg_pre/1013.25),0.1902587519025)));
                        /* ************************** */
                    }
                    CNT[0] = 0;
                    CNT[1] = 0;
                    CNT[2] = 0;
                    CNT[3] = 0;
                    IMUUPDATE = true;
                }

                if (IMUUPDATE) {
                    /* ********* 排除加速度异常值 ***************** */
                    if ((fsf0[0] == 0 || fsf0[1] == 0 || fsf0[2] == 0) && INSCNT <= 100) {             // 排除加速度异常值
                        fsf0 = avg_fsf;
                    } else {
                        for (int i = 0; i < 3; i++) {
                            if (Math.abs(fsf0[i] - avg_fsf[i]) > 2) {
                                avg_fsf[i] = fsf0[i];
                            } else {
                                fsf0[i] = avg_fsf[i];
                            }
                        }
                    }
                    /* *********************************************** */

                    avg_wib = lpFilterW.update(avg_wib);        // 角速度低通滤波
                    avg_fsf = lpFilterF.update(avg_fsf);        // 加速度低通滤波
                    avg_mag = lpFilterM.update(avg_mag);        // 磁力计低通滤波

                    INSCNT++;       // 导航计数+1
                    IMUUPDATE = false;  // 清除IMU更新标志

                    /*===========  解算  ==========*/
                    if (INSCNT >= 200) {        // 等待滤波结果稳定再解算(2s) Android传感器竟然也有启动时间
                        k++;
                        if (navState == IDLESTATE) {
                            if (k >= 19) {          // 以较慢的频率刷新传感器数据
                                Message msg1 = msgHandler.obtainMessage();
                                msg1.arg1 = DISPMSG_SENSOR;
                                msgHandler.sendMessage(msg1);
                                k = 0;
                            }
                        } else if (navState == ALIGNSTATE || navState == CPNAVRDY) {       // 航姿+Align
                            att = AHRS_Update(avg_wib, avg_fsf, avg_mag);
                            if (k >= 19) {
                                Message msg1 = msgHandler.obtainMessage();
                                msg1.arg1 = DISPMSG_SENSOR;
                                msgHandler.sendMessage(msg1);
                                Message msg2 = msgHandler.obtainMessage();
                                msg2.arg1 = DISPMSG_AHRS;
                                msgHandler.sendMessage(msg2);
                                k = 0;
                            }
                            if (saveOn) {
                                psinsDataStore.writedata(avg_wib, avg_fsf, avg_mag, att, qnb, t);
                            }
                        } else if (navState == CPNAVSTATE) {        // 组合导航
                            avppx = KFAPP_TUpdate(avg_wib, avg_fsf);        // 时间更新
//                            if (SimulationON && INSCNT % 100 == 0) {        // 进入仿真量测状态
//                                gnssData[0] = 2.0;
//                                gnssData[1]++;
//                                gnssData[2] = pos0[0];
//                                gnssData[3] = pos0[1];
//                                gnssData[4] = pos0[2];
//                                gnssData[5] = vn0[0];
//                                gnssData[6] = vn0[1];
//                                gnssData[7] = vn0[2];
//                                xkpkk = KFAPP_MUpdate(gnssData);            // 量测更新
//                                Message msg1 = msgHandler.obtainMessage();
//                                msg1.arg1 = DISPMSG_XKPK;
//                                msgHandler.sendMessage(msg1);
//                            }
                            if (k >= 19) {
                                Message msg1 = msgHandler.obtainMessage();
                                msg1.arg1 = DISPMSG_SENSOR;
                                msgHandler.sendMessage(msg1);
                                Message msg2 = msgHandler.obtainMessage();
                                msg2.arg1 = DISPMSG_SINS;
                                msgHandler.sendMessage(msg2);
                                k = 0;
                            }
                            if (saveOn) {
                                psinsDataStore.writebindata(avg_wib, avg_fsf, avg_mag, gnssData, avppx, t);
                            }
                        }
                    }
                }
            }
        };
        naviTimer.schedule(IMUdataTask, 0,10);     // Start IMU data sampling
        t0_stp = System.currentTimeMillis();

        /* ******** 底部功能button响应函数 *********** */
        buttonListener = new View.OnClickListener() {
            @Override
            public void onClick(View v) {
                switch (v.getId())
                {
                    case R.id.btn_nav:          // 开始解算
                        if (navState==IDLESTATE) {
                            navState = ALIGNSTATE;
                            INSCNT = 0;
                            AHRS_Init(0.01);
                            t0_stp = System.currentTimeMillis();
                            btn_cop.setEnabled(true);
                            btn_nav.setText("停止航姿解算");
                        } else if (navState == ALIGNSTATE) {
                            navState = IDLESTATE;
                            btn_cop.setEnabled(false);
                            btn_nav.setText("开始航姿解算");
                        }
                        break;
                    case R.id.btn_save:         // 数据保存
                        if (navState == CPNAVSTATE) {           // 不再保存航姿数据
                            if (saveOn == false) {
                                btn_save.setText("停止保存");
                                psinsDataStore.open();
                                saveOn = true;
                            } else {
                                btn_save.setText("开始保存");
                                saveOn = false;
                                psinsDataStore.close();
                            }
                        }
                        break;
                    case R.id.btn_cop:          // 组合导航
                        btn_nav.setEnabled(false);
                        btn_cop.setEnabled(false);
                        btn_about.setEnabled(false);
                        btn_save.setEnabled(true);
//                        if (!SimulationON) {
                            navState = CPNAVRDY;
                            locationManager.requestLocationUpdates(LocationManager.GPS_PROVIDER,
                                    500, 0, locationListener);      // 加入监听
//                        } else {
//                            locationManager.removeUpdates(locationListener);
//                            KFAPP_Init(vn0, pos0);
//                            readKFParas();      // 读取设置的KF参数
//                            btn_save.setText("停止保存");
//                            psinsDataStore.open();
//                            saveOn = true;
//                            navState = CPNAVSTATE;
//                        }
                        break;
                    case R.id.btn_about:         // 关于软件
                        Intent intent = new Intent( MainActivity.this, AboutActivity.class);
                        startActivity(intent);
                        break;
                    case R.id.btn_dat:          // 打开数据文件夹
                        Intent intentF = new Intent(Intent.ACTION_GET_CONTENT);
                        intentF.addCategory(Intent.CATEGORY_OPENABLE);
                        intentF.setDataAndType(Uri.parse(String.valueOf(MainActivity.this.getExternalFilesDir(null))), "resource/folder");
                        startActivity(Intent.createChooser(intentF, "Open folder"));
                        break;
                    case R.id.btn_share:        // 打开微信分享界面
                        if (PSINSDataStore.isCreated)
                        {

                        }
                        else
                            Toast.makeText(MainActivity.this,"微信分享仅对最新保存文件有效，以往数据请点击左侧文件分享", Toast.LENGTH_LONG).show();
                        break;
                    case R.id.btn_kfPara:
                        Intent intent1 = new Intent(MainActivity.this, KFParaSetting.class);
                        startActivity(intent1);
                        break;
                }
            }
        };
        btn_nav.setOnClickListener(buttonListener);         // 为四个按钮注册监听器
        btn_save.setOnClickListener(buttonListener);
        btn_cop.setOnClickListener(buttonListener);
        btn_about.setOnClickListener(buttonListener);
        btn_data.setOnClickListener(buttonListener);
        btn_share.setOnClickListener(buttonListener);
        btn_kfPara.setOnClickListener(buttonListener);
    }

    // 消息处理类
    private class MsgHandler extends Handler{
        @Override
        public void handleMessage(@NonNull Message msg) {
            super.handleMessage(msg);
            switch (msg.arg1){
                case DISPMSG_SENSOR:
                    SensorDataDisp(avg_wib, avg_fsf, avg_mag, avg_pre, presAlt, t);
                    break;
                case DISPMSG_AHRS:
                    AHRSDataDisp(att);
                    break;
                case DISPMSG_SINS:
                    AHRSDataDisp(avppx);
                    break;
            }
        }
    }

    @Override
    protected void onDestroy() {
        super.onDestroy();
        psinsDataStore.close();
    }

    /************************************************
     *   读取设置的KF参数
     ************************************************/
    private void readKFParas()
    {
        SharedPreferences sharedPreferences_kfPara = this.getSharedPreferences("com.tmrnavi.myapplication.KFPARAS",
                Context.MODE_PRIVATE);              // 初始化Share Preference
        if (sharedPreferences_kfPara.getFloat("Pk" + k, -1)>0) {
            for (int k = 0; k < 15; k++) {
                kfParas[k] = sharedPreferences_kfPara.getFloat("Pk" + k, -1);
            }
            for (int k = 0; k < 6; k++) {
                kfParas[k + 15] = sharedPreferences_kfPara.getFloat("Qk" + k, -1);
                kfParas[k + 21] = sharedPreferences_kfPara.getFloat("Rk" + k, -1);
            }
            KFAPP_SetParas(kfParas);
        }
    }

    /************************************************
     *   更新传感器数据显示
     ************************************************/
    private void SensorDataDisp(float[] w, float[] f, float[] m, float baro, float presAlt, float t)
    {
        tv_time.setText(String.format("%8.3f",t));
        tv_gyro_x.setText(String.format("%8.4f",w[0]));
        tv_gyro_y.setText(String.format("%8.4f",w[1]));
        tv_gyro_z.setText(String.format("%8.4f",w[2]));
        tv_acce_x.setText(String.format("%8.4f",f[0]));
        tv_acce_y.setText(String.format("%8.4f",f[1]));
        tv_acce_z.setText(String.format("%8.4f",f[2]));
        tv_magn_x.setText(String.format("%8.4f",m[0]));
        tv_magn_y.setText(String.format("%8.4f",m[1]));
        tv_magn_z.setText(String.format("%8.4f",m[2]));
        tv_baro.setText(String.format("%8.4f",baro));
        tv_presAlt.setText(String.format("%8.4f",presAlt));
    }

    /************************************************
     *   更新航姿或导航结果数据显示
     ************************************************/
    private void AHRSDataDisp(float[] data) {
        tv_atti_x.setText(String.format("%8.4f",data[0]));
        tv_atti_y.setText(String.format("%8.4f",data[1]));
        tv_atti_z.setText(String.format("%8.4f",data[2]));
    }
    private void AHRSDataDisp(double[] data) {
        tv_atti_x.setText(String.format("%8.4f",data[0]));
        tv_atti_y.setText(String.format("%8.4f",data[1]));
        tv_atti_z.setText(String.format("%8.4f",data[2]));
        tv_veln_x.setText(String.format("%8.4f",data[3]));
        tv_veln_y.setText(String.format("%8.4f",data[4]));
        tv_veln_z.setText(String.format("%8.4f",data[5]));
        tv_posi_x.setText(String.format("%8.4f",(data[7]-pos0[1])*R_eth[1]));
        tv_posi_y.setText(String.format("%8.4f",(data[6]-pos0[0])*R_eth[0]));
        tv_posi_z.setText(String.format("%8.4f",data[8]));
    }

    /************************************************
     *   更新GNSS位置数据显示
     ************************************************/
    private void GNSSDataDisp(double[] gnssData)
    {
        tv_gnss_time.setText(String.valueOf(gnssData[1]));
        tv_gnss_lat.setText(String.format("%.5f",gnssData[2]/DEG));
        tv_gnss_lng.setText(String.format("%.5f",gnssData[3]/DEG));
        tv_gnss_alt.setText(String.format("%.5f",gnssData[4]));
    }

    /************************************************
     *   更新GNSS速度数据显示
     ************************************************/
    private void GNSSVelDisp(double[] gnssVel)
    {
        tv_gnss_vhor.setText(String.format("%.4f", gnssVel[0]));
        tv_gnss_head.setText(String.format("%.4f", gnssVel[1]));
    }

    /*****************************************************
     * C++ 导航解算函数接口
     *****************************************************/
    public native void AHRS_Init(double ts);        // 航姿解算初始化
    public native float[] AHRS_Update(float[] wib, float[] fsf, float[] mag);   // 航姿解算更新
    public native void KFAPP_Init(double[] vn0, double[] pos0);         // KFApp初始化
    public native double[] KFAPP_SetParas(float[] kfParas);              // KF 参数设置
    public native double[] KFAPP_TUpdate(float[] wib, float[] fsf);     // KF 时间更新
    public native double[] KFAPP_MUpdate(double[] gdata0);              // KF 量测更新

}